
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_motors_multicopter.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "gp_motors.h"
/*-----------------------------------macro------------------------------------*/
#ifndef GP_MOTORS_DENSITY_COMP
#define GP_MOTORS_DENSITY_COMP 1
#endif

#define GP_MOTORS_YAW_HEADROOM_DEFAULT  200
#define GP_MOTORS_THST_EXPO_DEFAULT     0.65f   // set to 0 for linear and 1 for second order approximation
#define GP_MOTORS_THST_HOVER_DEFAULT    0.35f   // the estimated hover throttle, 0 ~ 1
#define GP_MOTORS_THST_HOVER_TC         10.0f   // time constant used to update estimated hover throttle, 0 ~ 1
#define GP_MOTORS_THST_HOVER_MIN        0.125f  // minimum possible hover throttle
#define GP_MOTORS_THST_HOVER_MAX        0.6875f // maximum possible hover throttle
#define GP_MOTORS_SPIN_MIN_DEFAULT      0.15f   // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
#define GP_MOTORS_SPIN_MAX_DEFAULT      0.95f   // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
#define GP_MOTORS_SPIN_ARM_DEFAULT      0.10f   // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
#define GP_MOTORS_BAT_VOLT_MAX_DEFAULT  0.0f    // voltage limiting max default
#define GP_MOTORS_BAT_VOLT_MIN_DEFAULT  0.0f    // voltage limiting min default (voltage dropping below this level will have no effect)
#define GP_MOTORS_BAT_CURR_MAX_DEFAULT  0.0f    // current limiting max default
#define GP_MOTORS_BAT_CURR_TC_DEFAULT   5.0f    // Time constant used to limit the maximum current
#define GP_MOTORS_BATT_VOLT_FILT_HZ     0.5f    // battery voltage filtered at 0.5hz
#define GP_MOTORS_SLEW_TIME_DEFAULT     0.0f    // slew rate limit for thrust output
#define GP_MOTORS_SAFE_TIME_DEFAULT     1.0f    // Time for the esc when transitioning between zero pwm to minimum

// spool definition
#define GP_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f    // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.

/*----------------------------------typedef-----------------------------------*/
// enum values for HOVER_LEARN parameter
typedef enum {
    HOVER_LEARN_DISABLED = 0,
    HOVER_LEARN_ONLY = 1,
    HOVER_LEARN_AND_SAVE = 2
} HoverLearn;

typedef struct
{
    Motors_HandleTypeDef tMotors;

    // 参数
    int16_t         _yaw_headroom;          // yaw control is given at least this pwm range
    float           _thrust_curve_expo;     // curve used to linearize pwm to thrust conversion.  set to 0 for linear and 1 for second order approximation
    float           _slew_up_time;          // throttle increase slew limitting
    float           _slew_dn_time;          // throttle decrease slew limitting
    float           _safe_time;             // Time for the esc when transitioning between zero pwm to minimum
    float           _spin_min;              // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
    float           _spin_max;              // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
    float           _spin_arm;              // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
    float           _batt_voltage_max;      // maximum voltage used to scale lift
    float           _batt_voltage_min;      // minimum voltage used to scale lift
    float           _batt_current_max;      // current over which maximum throttle is limited
    float           _batt_current_time_constant;    // Time constant used to limit the maximum current
    int8_t          _batt_idx;              // battery index used for compensation
    int16_t         _pwm_min;               // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
    int16_t         _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
    float           _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1
    int8_t          _throttle_hover_learn;  // enable/disabled hover thrust learning
    int8_t          _disarm_disable_pwm;    // disable PWM output while disarmed
    float           _yaw_servo_angle_max_deg; // Maximum lean angle of yaw servo in degrees. This is specific to tricopter
    float           _spool_up_time;         // time to spool motors to min throttle
    float           _boost_scale;           // scaling for booster motor throttle

    // motor output variables
    bool            motor_enabled[GP_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled
    int16_t         _throttle_radio_min;        // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN)
    int16_t         _throttle_radio_max;        // maximum PWM from RC input's throttle channel (i.e. maximum PWM input from receiver, RC3_MAX)

    // spool variables
    float           _spin_up_ratio;         // throttle percentage (0 ~ 1) between zero and throttle_min

    // battery voltage, current and air pressure compensation variables
    LowPassFilt     _batt_voltage_filt;     // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
    float           _lift_max;              // maximum lift ratio from battery voltage
    float           _throttle_limit;        // ratio of throttle limit between hover and maximum
    float           _throttle_thrust_max;   // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
    float           _disarm_safe_timer;     // Timer for the esc when transitioning between zero pwm to minimum

    // array of motor output values
    float           _actuator[GP_MOTORS_MAX_NUM_MOTORS];
}MotorsMC_HandleTypeDef;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void          MotorsMulticopter(MotorsMC_HandleTypeDef *pMotors, uint16_t loop_rate, uint16_t speed_hz);

//更新参数
void          MotorsMC_assign_param(MotorsMC_HandleTypeDef *pMotors);

// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
void          MotorsMC_set_yaw_headroom(MotorsMC_HandleTypeDef *pMotors, int16_t pwm);

// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets minimum and maximum pwm values that will be sent to the motors
void          MotorsMC_set_throttle_range(MotorsMC_HandleTypeDef *pMotors, int16_t radio_min, int16_t radio_max);

// update estimated throttle required to hover
void          MotorsMC_update_throttle_hover(MotorsMC_HandleTypeDef *pMotors, float dt);
static inline float MotorsMC_get_throttle_hover(MotorsMC_HandleTypeDef *pMotors) { return math_constrain_float(pMotors->_throttle_hover, GP_MOTORS_THST_HOVER_MIN, GP_MOTORS_THST_HOVER_MAX);}

// passes throttle directly to all motors for ESC calibration.
//   throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void          MotorsMC_set_throttle_passthrough_for_esc_calibration(MotorsMC_HandleTypeDef *pMotors, float throttle_input);

// get minimum pwm value that can be output to motors
int16_t       MotorsMC_get_pwm_output_min(MotorsMC_HandleTypeDef *pMotors);
// get maximum pwm value that can be output to motors
int16_t       MotorsMC_get_pwm_output_max(MotorsMC_HandleTypeDef *pMotors);

// get_lift_max - get maximum lift ratio - for logging purposes only
float         MotorsMC_get_lift_max(MotorsMC_HandleTypeDef *pMotors);

// get_batt_voltage_filt - get battery voltage ratio - for logging purposes only
float         MotorsMC_get_batt_voltage_filt(MotorsMC_HandleTypeDef *pMotors);

// get throttle limit imposed by battery current limiting.  This is a number from 0 ~ 1 where 0 means hover throttle, 1 means full throttle (i.e. not limited)
float         MotorsMC_get_throttle_limit(MotorsMC_HandleTypeDef *pMotors);

// returns maximum thrust in the range 0 to 1
float         MotorsMC_get_throttle_thrust_max(MotorsMC_HandleTypeDef *pMotors);

// return true if spool up is complete
bool          MotorsMC_spool_up_complete(MotorsMC_HandleTypeDef *pMotors);

// output a thrust to all motors that match a given motor
// mask. This is used to control tiltrotor motors in forward
// flight. Thrust is in the range 0 to 1
//void          MotorsMC_output_motor_mask(MotorsMC_HandleTypeDef *pMotors, float thrust, uint8_t mask, float rudder_dt);

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
//uint16_t      MotorsMC_get_motor_mask();

// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool          MotorsMC_check_mot_pwm_params(MotorsMC_HandleTypeDef *pMotors);

// converts desired thrust to linearized actuator output in a range of 0~1
float         MotorsMC_thrust_to_actuator(MotorsMC_HandleTypeDef *pMotors, float thrust_in);

// run spool logic
void          MotorsMC_output_logic(MotorsMC_HandleTypeDef *pMotors);

// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float         MotorsMC_get_current_limit_max_throttle(MotorsMC_HandleTypeDef *pMotors);

// update the throttle input filter
void          MotorsMC_update_throttle_filter(MotorsMC_HandleTypeDef *pMotors);

// update_lift_max_from_batt_voltage - used for voltage compensation
void          MotorsMC_update_lift_max_from_batt_voltage(MotorsMC_HandleTypeDef *pMotors);

// return gain scheduling gain based on voltage and air density
float         MotorsMC_get_compensation_gain(MotorsMC_HandleTypeDef *pMotors);

// convert actuator output (0~1) range to pwm range
int16_t       MotorsMC_output_to_pwm(MotorsMC_HandleTypeDef *pMotors, float _actuator_output);

// adds slew rate limiting to actuator output if MOT_SLEW_TIME > 0 and not shutdown
void          MotorsMC_set_actuator_with_slew(MotorsMC_HandleTypeDef *pMotors, float* actuator_output, float input);

// gradually increase actuator output to ground idle
float         MotorsMC_actuator_spin_up_to_ground_idle(MotorsMC_HandleTypeDef *pMotors);

// apply any thrust compensation for the frame
//void        MotorsMC_thrust_compensation(MotorsMC_HandleTypeDef *pMotors) {}

// output booster throttle, if any
void          MotorsMC_output_boost_throttle(MotorsMC_HandleTypeDef *pMotors);

// output roll/pitch/yaw/thrust
void          MotorsMC_output_rpyt(MotorsMC_HandleTypeDef *pMotors);

// save parameters as part of disarming
void          MotorsMC_save_params_on_disarm(MotorsMC_HandleTypeDef *pMotors);

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
static inline uint16_t MotorsMC_get_motor_mask() { return srv_channels_get_output_channel_mask(k_boost_throttle); }

// update_throttle_range - update throttle endpoints
void MotorsMC_update_throttle_range(MotorsMC_HandleTypeDef *pMotors);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



